/********** System Includes ***************************************************/

#include <sys/time.h>

/********** Local Includes ****************************************************/

#include "rovio.h"

/********** Namespaces ********************************************************/

using namespace std;

/********** Local Static Function Prototypes **********************************/

/**
 * A static helper function that returns the time with millisecond precision.
 */
long msec();

/********** Public Methods ****************************************************/

Rovio::Rovio(string url, int defaultSpeed) :
	m_url(url),
	m_defaultSpeed(defaultSpeed)
{
	m_curl = curl_easy_init();
}

Rovio::~Rovio() {
	// TODO Why can't I clean this up?
	//delete curl;
}

bool Rovio::saveImage(const string &filename) {
	if (m_curl) {
		long start = msec();
		FILE *file = fopen(filename.c_str(), "wb");
		
		if (!file) {
			return false;
		}
		
		stringstream command;
		command << m_url;
		command << "/Jpeg/CamImg0.jpg";
		
		curl_easy_setopt(m_curl, CURLOPT_WRITEFUNCTION, NULL);
		curl_easy_setopt(m_curl, CURLOPT_WRITEDATA, file);
		curl_easy_setopt(m_curl, CURLOPT_URL, command.str().c_str());
		
		CURLcode result = curl_easy_perform(m_curl);
		fclose(file);
		
		std::cout << "Elapsed time: " << msec() - start << std::endl;
		return result == 0;
    }
	
	return false;
}

CURLcode Rovio::drive(DriveDirection dir, int speed) {
	stringstream command;

    command << m_url;
    command << "/rev.cgi?Cmd=nav&action=18&drive=";
    command << dir;
    command << "&speed=";
    command << speed;

    curl_easy_setopt(m_curl, CURLOPT_URL, command.str().c_str());
    return curl_easy_perform(m_curl);
}
	
CURLcode Rovio::drive(DriveDirection dir) {
	return drive(dir, m_defaultSpeed);
}
	
void Rovio::timeDrive(DriveDirection dir, int time, int speed) {
    long end = msec() + time;

    while (msec() < end) {
        drive(dir, speed);
    }
}

void Rovio::timeDrive(DriveDirection dir, int time) {
	timeDrive(dir, time, m_defaultSpeed);
}

void Rovio::driveForward(int time) {
	if (time >= 0) {
		timeDrive(DIR_FORWARD, time);
	} else {
		drive(DIR_FORWARD);
	}
}

void Rovio::turnLeft(int time) {
	if (time >= 0) {
		timeDrive(DIR_ROT_LEFT, time);
	} else {
		drive(DIR_ROT_LEFT);
	}
}

void Rovio::turnRight(int time) {
	if (time >= 0) {
		timeDrive(DIR_ROT_RIGHT, time);
	} else {
		drive(DIR_ROT_RIGHT);
	}
}

void Rovio::setDefaultSpeed(int defaultSpeed) {
	m_defaultSpeed = defaultSpeed;
}

int Rovio::defaultSpeed() const {
	return m_defaultSpeed;
}

/********** Local Static Functions ********************************************/

long msec() {
    struct timeval time;
    gettimeofday(&time, NULL);
    return ((time.tv_sec) * 1000 + time.tv_usec/1000.0) + 0.5;
}

/********** Program Entry *****************************************************/

/*int main(int, char **) {
	Rovio rovio("http://143.215.111.166");
	rovio.saveImage();
}*/
